/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdlib.h>
#include <string.h>

#include "command.h"

Waypt_t waypoint;
typedef struct {
  VehicleInfo_t info;
  SystemCharacter_t * systems [32];
} __attribute__ ((packed))Capabilities_t;
Capabilities_t capabilities;

#define TRUE 1
#define FALSE 0

uint8_t command[CMD_MAX_SIZE];
CommandQueue commandQueue;

int main(int argc, char *argv[])
{
  int sockfd, portno, clilen;
  struct sockaddr_in serv_addr, cli_addr;

  char * argument;  // junk to get rid of compilier warnings
  if(argc > 0)
   argument = argv[0];
  
  portno = 12345;

  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  if(sockfd < 0) {
    printf("ERROR: opening socket\n");
    exit(0);
  }

  bzero((char *) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  serv_addr.sin_addr.s_addr = INADDR_ANY;
  serv_addr.sin_port = htons(portno);
  if (bind(sockfd, (struct sockaddr *) &serv_addr,
        sizeof(serv_addr)) < 0) {
    printf("ERROR on binding\n");
    exit(0);
  }

  listen(sockfd,5);
  clilen = sizeof(cli_addr);

  while (1) {
    int n;
    unsigned int i;

    bzero(command,CMD_MAX_SIZE);
    n = read(sockfd,command,1599);
    if (n < 0) printf("ERROR reading from socket");
    if (n > 0) {
			printf("read %d bytes from socket\n",n);
      switch(CMD_CMD(command)) {
        case CMD_FP:
          for(i=0; i<CMD_SIZE(command)/sizeof(Waypt_t); i++) {
            CopyBufToStruct(&command[3+i*sizeof(Waypt_t)], waypoint);
            printf("lat: %f\tlon: %f\n", waypoint.pos.lat, waypoint.pos.lon);
          }
          break;
        case CMD_CAPABILITY:
          CopyBufToStruct(&command[3], capabilities);
          printf("%s: \n", capabilities.info.id);
          break;
      }
    }
  } /* end of while */
  return 0; /* we never get here */
}
